#include "uni_motor.h"
#include "uni_servo.h"
#include "WonderK210.h"

#define BORD_RATE 115200//设定串口波特率为115200
UNI_MOTOR motor1(1, true);//定义编码电机1
UNI_MOTOR motor2(2, true);//定义编码电机2
UNI_MOTOR motor3(3, true);//定义编码电机3
UNI_MOTOR motor4(4, true);//定义编码电机4
WonderK210* wk;//接收控制器
Find_Box_Msg_st* result;//消息存储变量
UNI_SERVO_S servo(4, 6, 7, 10);//声明舵机4,6,7,10处的舵机
int stepper_speed = 4800; //设定步进电机速度

void setup() {
  delay(500);//开机等待500毫秒
  Serial.begin(BORD_RATE);//打开串口
  stepper_init();//步进电机引脚初始化
  car_init();//麦克纳姆四轮小车四个电机引脚初始化
  servo_init();//初始化舵机动作
  vision_init(); //视觉模块初始化
  //stepmove_test();//步进电机运动测试
  //car_action_test(); //小车运动测试
  servo_action_test(); //舵机动作测试
}

void loop() {
  qr_detect(); //实时检测接收到视觉模块发送的信息
  //car_update(); //实时更新并保持小车四个电机的位置
}
